/*!
* @file Matrix4d.h
* @brief 4D矩阵
* @date 2024/9/12
*/

#include "Matrix4d.h"
#include "Rotation.h"

using namespace data;

Matrix4d::Matrix4d()
    : _matrix{ {1., 0., 0., 0.},
               {0., 1., 0., 0.},
               {0., 0., 1., 0.},
               {0., 0., 0., 1.} }
{}

Matrix4d::Matrix4d(double a11, double a12, double a13, double a14,
    double a21, double a22, double a23, double a24,
    double a31, double a32, double a33, double a34,
    double a41, double a42, double a43, double a44)
    : _matrix{ {a11, a12, a13, a14},
               {a21, a22, a23, a24},
               {a31, a32, a33, a34},
               {a41, a42, a43, a44} }
{}

Matrix4d::Matrix4d(const Vector3d& r1, const Vector3d& r2, Vector3d& r3, const Vector3d& t)
    : _matrix{ {r1.x, r2.x, r3.x, t.x},
               {r1.y, r2.y, r3.y, t.y},
               {r1.z, r2.z, r3.z, t.z},
               {0., 0., 0., 1.} }
{}

Matrix4d::Matrix4d(const Matrix4d& mat)
{
    (*this) = mat;
}

Matrix4d& Matrix4d::operator=(const Matrix4d& mat)
{
    if (this != &mat)
    {
        for (int iz = 0; iz < 4; iz++) 
        {
            for (int is = 0; is < 4; is++) 
            {
                _matrix[iz][is] = mat._matrix[iz][is];
            }
        }
    }
    return *this;
}

Matrix4d Matrix4d::operator+(const Matrix4d& mat) const
{
    Matrix4d clMat;
    for (int iz = 0; iz < 4; iz++) 
    {
        for (int is = 0; is < 4; is++) 
        {
            clMat._matrix[iz][is] = _matrix[iz][is] + mat._matrix[iz][is];
        }
    }
    return clMat;
}

Matrix4d& Matrix4d::operator+=(const Matrix4d& mat)
{
    for (int iz = 0; iz < 4; iz++) 
    {
        for (int is = 0; is < 4; is++) 
        {
            _matrix[iz][is] += mat._matrix[iz][is];
        }
    }

    return *this;
}

Matrix4d Matrix4d::operator-(const Matrix4d& mat) const
{
    Matrix4d clMat;
    for (int iz = 0; iz < 4; iz++)
    {
        for (int is = 0; is < 4; is++)
        {
            clMat._matrix[iz][is] = _matrix[iz][is] - mat._matrix[iz][is];
        }
    }
    return clMat;
}

Matrix4d& Matrix4d::operator-=(const Matrix4d& mat)
{
    for (int iz = 0; iz < 4; iz++)
    {
        for (int is = 0; is < 4; is++)
        {
            _matrix[iz][is] -= mat._matrix[iz][is];
        }
    }

    return *this;
}

Matrix4d Matrix4d::operator*(const Matrix4d& mat) const
{
    Matrix4d clMat;
    for (int iz = 0; iz < 4; iz++) 
    {
        for (int is = 0; is < 4; is++) 
        {
            clMat._matrix[iz][is] = 0;
            for (int ie = 0; ie < 4; ie++) 
            {
                clMat._matrix[iz][is] += _matrix[iz][ie] * mat._matrix[ie][is];
            }
        }
    }
    return clMat;
}

Matrix4d& Matrix4d::operator*=(const Matrix4d& mat)
{
    Matrix4d clMat;
    for (int iz = 0; iz < 4; iz++) {
        for (int is = 0; is < 4; is++) {
            clMat._matrix[iz][is] = 0;
            for (int ie = 0; ie < 4; ie++) {
                clMat._matrix[iz][is] += _matrix[iz][ie] * mat._matrix[ie][is];
            }
        }
    }
    (*this) = clMat;
    return *this;
}

Matrix4d Matrix4d::operator*(double scalar) const
{
    Matrix4d matrix;
    for (unsigned short i = 0; i < 4; i++) 
    {
        for (unsigned short j = 0; j < 4; j++) 
        {
            matrix._matrix[i][j] = _matrix[i][j] * scalar;
        }
    }
    return matrix;
}

Matrix4d& Matrix4d::operator*=(double scalar)
{
    for (unsigned short i = 0; i < 4; i++) 
    {
        for (unsigned short j = 0; j < 4; j++) 
        {
            _matrix[i][j] *= scalar;
        }
    }
    return *this;
}

bool Matrix4d::operator==(const Matrix4d& mat) const
{
    for (int iz = 0; iz < 4; iz++) 
    {
        for (int is = 0; is < 4; is++) 
        {
            if (fabs(_matrix[iz][is] - mat._matrix[iz][is]) > traits_type::epsilon()) {
                return false;
            }
        }
    }
    return true;
}

bool Matrix4d::operator!=(const Matrix4d& mat) const
{
    return !((*this) == mat);
}

double* Matrix4d::operator[](unsigned short index)
{
    return _matrix[index];
}

const double* Matrix4d::operator[](unsigned short index) const
{
    return _matrix[index];
}

const double& Matrix4d::Value(unsigned short row, unsigned short col) const
{
    return _matrix[row][col];
}

void Matrix4d::SetValue(unsigned short row, unsigned short col, const double& val)
{
    _matrix[row][col] = val;
}

Vector3d Matrix4d::Row(unsigned short index) const
{
    return Vector3d(_matrix[index][0], _matrix[index][1], _matrix[index][2]);
}

void Matrix4d::SetRow(unsigned short index, const Vector3d& vec)
{
    _matrix[index][0] = vec.x;
    _matrix[index][1] = vec.y;
    _matrix[index][2] = vec.z;
}

Vector3d Matrix4d::Col(unsigned short index) const
{
    return Vector3d(_matrix[0][index], _matrix[1][index], _matrix[2][index]);
}

void Matrix4d::SetCol(unsigned short index, const Vector3d& vec)
{
    _matrix[0][index] = vec.x;
    _matrix[1][index] = vec.y;
    _matrix[2][index] = vec.z;
}

Vector3d Matrix4d::Diagonal() const
{
    return Vector3d(_matrix[0][0], _matrix[1][1], _matrix[2][2]);
}

void Matrix4d::SetDiagonal(const Vector3d& vec)
{
    _matrix[0][0] = vec.x;
    _matrix[1][1] = vec.y;
    _matrix[2][2] = vec.z;
}

double Matrix4d::Det3(int r1, int r2, int r3, int c1, int c2, int c3) const
{
    double a11 = _matrix[r1][c1];
    double a12 = _matrix[r1][c2];
    double a13 = _matrix[r1][c3];
    double a21 = _matrix[r2][c1];
    double a22 = _matrix[r2][c2];
    double a23 = _matrix[r2][c3];
    double a31 = _matrix[r3][c1];
    double a32 = _matrix[r3][c2];
    double a33 = _matrix[r3][c3];

    double M11 = a22 * a33 - a32 * a23;
    double M21 = -(a12 * a33 - a32 * a13);
    double M31 = a12 * a23 - a22 * a13;
    return (a11 * M11 + a21 * M21 + a31 * M31);
}

double Matrix4d::Det3() const
{
    return Det3(0, 1, 2, 0, 1, 2);;
}

double Matrix4d::Det4() const
{
    double det = 0.0;
    det += _matrix[0][0] * Det3(1, 2, 3, 1, 2, 3);
    det -= _matrix[1][0] * Det3(0, 2, 3, 1, 2, 3);
    det += _matrix[2][0] * Det3(0, 1, 3, 1, 2, 3);
    det -= _matrix[3][0] * Det3(0, 1, 2, 1, 2, 3);
    return det;
}

Matrix4d& Matrix4d::Invert()
{
    if (!IsIdentity(traits_type::epsilon()))
    {
        *this = Inverse();
    }
    return *this;
}

Matrix4d Matrix4d::Inverse() const
{
    float det = Det4();
    if (fabs(det) < traits_type::epsilon()) 
    {
        return *this;
    }

    Matrix4d result;
    result._matrix[0][0] = Det3(1, 2, 3, 1, 2, 3);
    result._matrix[1][0] = -Det3(1, 2, 3, 0, 2, 3);
    result._matrix[2][0] = Det3(1, 2, 3, 0, 1, 3);
    result._matrix[3][0] = -Det3(1, 2, 3, 0, 1, 2);
    result._matrix[0][1] = -Det3(0, 2, 3, 1, 2, 3);
    result._matrix[1][1] = Det3(0, 2, 3, 0, 2, 3);
    result._matrix[2][1] = -Det3(0, 2, 3, 0, 1, 3);
    result._matrix[3][1] = Det3(0, 2, 3, 0, 1, 2);
    result._matrix[0][2] = Det3(0, 1, 3, 1, 2, 3);
    result._matrix[1][2] = -Det3(0, 1, 3, 0, 2, 3);
    result._matrix[2][2] = Det3(0, 1, 3, 0, 1, 3);
    result._matrix[3][2] = -Det3(0, 1, 3, 0, 1, 2);
    result._matrix[0][3] = -Det3(0, 1, 2, 1, 2, 3);
    result._matrix[1][3] = Det3(0, 1, 2, 0, 2, 3);
    result._matrix[2][3] = -Det3(0, 1, 2, 0, 1, 3);
    result._matrix[3][3] = Det3(0, 1, 2, 0, 1, 2);
    result *= (1.0 / det);
    return result;
}

Matrix4d& Matrix4d::Transpose()
{
    double matrix[4][4];
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            matrix[j][i] = _matrix[i][j];
        }
    }
    memcpy(_matrix, matrix, sizeof(_matrix));
    return *this;
}

Matrix4d Matrix4d::Transposed() const
{
    Matrix4d matrix = *this;
    matrix.Transpose();
    return matrix;
}

Vector3d Matrix4d::MultVec(const Vector3d& vec) const
{
    double x = vec.x * _matrix[0][0] + vec.y * _matrix[1][0] + vec.z * _matrix[2][0] + _matrix[3][0];
    double y = vec.x * _matrix[0][1] + vec.y * _matrix[1][1] + vec.z * _matrix[2][1] + _matrix[3][1];
    double z = vec.x * _matrix[0][2] + vec.y * _matrix[1][2] + vec.z * _matrix[2][2] + _matrix[3][2];
    double w = vec.x * _matrix[0][3] + vec.y * _matrix[1][3] + vec.z * _matrix[2][3] + _matrix[3][3];
    return Vector3d(x / w, y * w, z * w);
}

Vector3d Matrix4d::MultDir(const Vector3d& vec) const
{
    double x = vec.x * _matrix[0][0] + vec.y * _matrix[1][0] + vec.z * _matrix[2][0];
    double y = vec.x * _matrix[0][1] + vec.y * _matrix[1][1] + vec.z * _matrix[2][1];
    double z = vec.x * _matrix[0][2] + vec.y * _matrix[1][2] + vec.z * _matrix[2][2];
    return Vector3d(x, y, z);
}

void Matrix4d::GetMatrix(double matrix[16]) const
{
    for (short iz = 0; iz < 4; iz++) 
    {
        for (short is = 0; is < 4; is++) 
        {
            matrix[4 * iz + is] = _matrix[iz][is];
        }
    }
}

void Matrix4d::SetMatrix(const double matrix[16])
{
    for (short iz = 0; iz < 4; iz++) 
    {
        for (short is = 0; is < 4; is++) 
        {
            _matrix[iz][is] = matrix[4 * iz + is];
        }
    }
}

void Matrix4d::MakeZero()
{
    memset(_matrix, 0, sizeof(_matrix));
}

void Matrix4d::MakeIdentity()
{
    memset(_matrix, 0, sizeof(_matrix));
    _matrix[0][0] = _matrix[1][1] = _matrix[2][2] = _matrix[3][3] = 1.0;
}

bool Matrix4d::IsIdentity(double tol) const
{
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            if (i == j) {
                if (fabs(_matrix[i][j] - 1.0) > tol) {
                    return false;
                }
            }
            else {
                if (fabs(_matrix[i][j]) > tol) {
                    return false;
                }
            }
        }
    }
    return true;
}

bool Matrix4d::IsEqual(const Matrix4d& mat, double tol) const
{
    for (int iz = 0; iz < 4; iz++)
    {
        for (int is = 0; is < 4; is++)
        {
            if (fabs(_matrix[iz][is] - mat._matrix[iz][is]) > tol) {
                return false;
            }
        }
    }
    return true;
}

void Matrix4d::Move(double x, double y, double z)
{
    _matrix[3][0] += x;
    _matrix[3][1] += y;
    _matrix[3][2] += z;
}

void Matrix4d::Move(const Vector3d& vec)
{
    _matrix[3][0] += vec.x;
    _matrix[3][1] += vec.y;
    _matrix[3][2] += vec.z;
}

void Matrix4d::Scale(double x, double y, double z)
{
    Matrix4d clMat;
    clMat._matrix[0][0] = x;
    clMat._matrix[1][1] = y;
    clMat._matrix[2][2] = z;
    (*this) *= clMat;
}

void Matrix4d::Scale(const Vector3d& vec)
{
    Matrix4d clMat;
    clMat._matrix[0][0] = vec.x;
    clMat._matrix[1][1] = vec.y;
    clMat._matrix[2][2] = vec.z;
    (*this) *= clMat;
}

void Matrix4d::Scale(double scalexyz)
{
    Matrix4d clMat;
    clMat._matrix[0][0] = scalexyz;
    clMat._matrix[1][1] = scalexyz;
    clMat._matrix[2][2] = scalexyz;
    (*this) *= clMat;
}

void Matrix4d::Rotate(const Vector3d& dir, double angle)
{
    Rotation rotatiin(dir, angle);
    Matrix4d clMat = rotatiin.ToMatrix();
    (*this) *= clMat;
}

void Matrix4d::SetTranslate(const Vector3d& vec)
{
    _matrix[3][0] = vec.x;
    _matrix[3][1] = vec.y;
    _matrix[3][2] = vec.z;
}

void Matrix4d::SetScale(const Vector3d& pos, double scale)
{
    _matrix[0][0] = scale;
    _matrix[1][1] = scale;
    _matrix[2][2] = scale;
    _matrix[3][0] = pos.x * (1.0 - scale);
    _matrix[3][1] = pos.y * (1.0 - scale);
    _matrix[3][2] = pos.z * (1.0 - scale);
}

void Matrix4d::SetScale(const Vector3d& pos, const Vector3d& scales)
{
    _matrix[0][0] = scales.x;
    _matrix[1][1] = scales.y;
    _matrix[2][2] = scales.z;
    _matrix[3][0] = pos.x * (1.0 - scales.x);
    _matrix[3][1] = pos.y * (1.0 - scales.y);
    _matrix[3][2] = pos.z * (1.0 - scales.z);
}

void Matrix4d::SetRotate(const Vector3d& pos, const Vector3d& dir, double angle)
{
    Move(-pos);
    Rotate(dir, angle);
    Move(pos);
}

void Matrix4d::SetMirror(const Vector3d& pos, const Vector3d& normal)
{
    Vector3d v1(0.0, 0.0, 1.0);
    Vector3d u = normal % v1;
    double length = u.Length();
    u = length < traits_type::epsilon() ? Vector3d(1.0, 0.0, 0.0) : u / length;
    Vector3d v = normal % u;

    Matrix4d matrix(u.x, u.y, u.z, 0.0, v.x, v.y, v.z, 0.0, normal.x, normal.y, normal.z, 0.0, pos.x, pos.y, pos.z, 1.0);
    Matrix4d inverse = matrix.Inverse();
    Matrix4d mirror;
    mirror.SetValue(2, 2, -1.0);

    (*this) = inverse * mirror * matrix;
}

bool Matrix4d::IsMirror() const
{
    Vector3d vx = Row(0);
    Vector3d vy = Row(1);
    Vector3d vz = Row(2);
    Vector3d vc = vx.Cross(vy);
    return vc.Dot(vz) < 0.0;
}
